#include <avr/io.h>
#include <avr/interrupt.h>
#include <stdlib.h>
#include <avr/interrupt.h>
#include <inttypes.h>
#include <util/delay.h>
#include "sonar.h"
#include "radio.h"


#define UP 0
#define DOWN 1
#define STABLE 2
#define DESIRED_DISTANCE 35
#define DESIRED_HEIGHT 35
#define COUNTER_SPIN 0.2
#define HORZ_MULTIPLIER 0.1 
#define HOR_DEADZONE 5

unsigned int overflows = 0;
int direction = UP;
int dist_to_ground;
int current_error = 0;
int previous_error = 0;
int derivative_error = 0;
int delta_error;
int turning_right = 0;
int turning_left = 0;
//int integral_error = 0;
int kp = 1; //proportional constant value
//int ki = 1; //integral constant value
int kd = -3;
int output;
int dead_zone = 1;
int16_t power;


/* 
 * Initialize the timers to provide drive signals to the motors.
 * Both timer 0 and timer 2 are required as there are 3 motors and only
 * two output registers for each timer. The fourth output register remains 
 * unused.
 */
void init_pwm_motor () {

	// Set the Timer0 to PWM Fast Mode
	TCCR0A = (_BV(WGM00)) | (_BV(WGM01)) | (_BV(COM0A1)) | (_BV(COM0B1));
	//Set the Timer on with no prescaler
	TCCR0B = (_BV(CS02));


	//Output register responsible for driving right motor
	OCR0A = 0xff;
	//Output register responsible for driving down motor
	OCR0B = 0xff;

	//Set Timer2 for fast pwm mode.
	TCCR2A = (_BV(WGM20)) | (_BV(WGM21)) | (_BV(COM2A1)) | (_BV(COM2B1));
	//Set the timer with no prescaler.
	TCCR2B = (_BV(CS22));

	//Output register responsible for driving left motor
	OCR2A = 0xff;
	//This register unused.
	OCR2B = 0xff;

	//Initialize ports for motor control
	/* Initialize pin for controlling down motor.
	 */
	DDRC |= _BV(5);


	/* As best I can tell this is the port that controls the left motor.
	 * This motor is going to be always driving forward, simply set it as such.
	 */


	DDRB |= _BV(6);
	/* As best I can tell this is the port that controls the right motor.
	 * This motor is going to be always driving forward, simply set it as such.
	 */
	DDRD |= _BV(9);

}

/*
void setDirection() {



	 if (direction == UP) {

	//  _delay_ms(10000);
	 // OCR0B = 0xff;
	 // PORTC |= (1>>PORTC5); //turn on motor direction 0
	  PORTC |= _BV(PORTC5);  //not sure what this does
	 } else if (direction == DOWN ){

	 // 	_delay_ms(10000);
	//	OCR0B = 0xff;
	  //	PORTC |= (1<<PORT5); //direction `
		PORTC &= ~(_BV(PORTC5));

	 } else {
	 	OCR0B = 0x00;
	 }
	 //radio_print("Task Next Called from setDirection\n");
	 Task_Next();
	
	

} 
*/
ISR(TIMER3_COMPA_vect)
{
	TCCR3A += 40000;
	overflows++;
}
void horz_pid(){
  int last_reading=0, error, range,far_count=0;
  int turn_val= 0, close_count=0, counter_spinning = 0;
  int standard_power = 170;
  unsigned int start, end, time_to_wait=0;
  double elapsed;
  start = overflows;

   for(;;){
   	if(overflows > 60000){
		overflows  = 0;
		start = 0;
		end = 0;
		}

	  end = overflows;
	  elapsed = ((double) (end - start)) / 200;
    
		/* is the time up for turning RIGHT*/
	  if(elapsed > time_to_wait && turning_right == 1)
	     {
			
			OCR0A = standard_power;      //restore full power to right motor
			OCR2A = standard_power / 2; // cut left motor to counterspin
		 	turning_right = 0;
			counter_spinning = 1;
			time_to_wait = (turn_val * COUNTER_SPIN);


		 }
 	
			/*is the time up for turning LEFT */
		if(elapsed > time_to_wait && turning_left == 1)
	     {
			OCR2A = standard_power;  //restore full power to left motor
			OCR0A = standard_power / 2; // cut right motor to counterspin
		 	turning_right = 0;
			counter_spinning = 1;
			time_to_wait = (turn_val * COUNTER_SPIN);
		 }
		
			/* restore normal power to motors */
		if(elapsed > time_to_wait && counter_spinning == 1)
		 {
			OCR2A = standard_power; //left motor back to standard
			OCR0A = standard_power; //right motor back to standard
		 	counter_spinning = 0;
		 }

	
	  
	 range = getDistanceFromWall();

/********** turn towards the wall ***********************/
	 
	  if(far_count == 0){
			if(range > DESIRED_DISTANCE + HOR_DEADZONE){
				last_reading = range;
				far_count++;
				close_count = 0;
			}
		}

		if(far_count > 0){
			if(range > last_reading){
				far_count++;
				last_reading = range;

			}
		}

		if(far_count == 3){
			 error = last_reading - (DESIRED_DISTANCE + HOR_DEADZONE);		
			 turn_val = (int)(error * HORZ_MULTIPLIER);
		   	 OCR0A = standard_power / 2;  // should be the number for the right motor
			 time_to_wait = turn_val;
			 start = overflows;
			 end = 0;
			 far_count = 0;
			 turning_right = 1;
		   }	
  	 

/*********** turn away from the wall *********************/

	 	if(close_count == 0){
			if(range < DESIRED_DISTANCE - HOR_DEADZONE){
				last_reading = range;
				close_count++;
				far_count = 0;
			}
		}
		
		if(close_count > 0){
		
			if(range > last_reading){
				close_count++;
				last_reading = range;
				}
			}
		
		if(close_count == 3){
			 error =  (DESIRED_DISTANCE + HOR_DEADZONE) - last_reading ;		
			 turn_val = (int)(error * HORZ_MULTIPLIER);
		   	 OCR2A = standard_power / 2; //should be the number for the left motor
			 time_to_wait = turn_val;
			 start = overflows;
			 end = 0;
			 close_count = 0;
			 turning_left = 1;
		   }
/********** fly straight ahead **************************/		   
		if(range >= DESIRED_DISTANCE - HOR_DEADZONE && range <= DESIRED_DISTANCE - HOR_DEADZONE){
			radio_print("Flying straight ahead, range is:%d \n", range); 
			OCR0A = standard_power;
			OCR2A = standard_power;
			}
			
 	Task_Next();
  	 }
}


void pid() {

	for(;;){

		//PID CONTROLLER
		//radio_print("pid task called!\n");
		//previous_error = current_error;
		dist_to_ground = getHeightfromGround(); //distance to ground
		radio_print("This is the distance to ground %d \n", dist_to_ground);

		current_error = DESIRED_HEIGHT - dist_to_ground;

//		integral_error = integral(current_error);
        derivative_error = previous_error - current_error;
		previous_error = current_error;
		output = (kp * current_error) + (kd * derivative_error);
//		radio_print("Output: %d Direction: %d Derivative Error : %d\r\n", output,direction, derivative_error);

		if(output < (-1 * dead_zone)) {
			//change direction
			if(((output*3) - 100)<-250){
				power = 250;
			} else {
				power = ((output*3) - 100);
			}

			PORTC &= ~(_BV(PORTC5)); // sets direction to down

		} else if ((output > (-1*dead_zone)) && (output < dead_zone)){

           	 OCR0B = 0x00; //height STABLE; turn off power to motor.
        } else {
				if(((output*10) + 100)>250){
					power = 250;
				} else {
				power = ((output*10) + 100);
				 PORTC |= _BV(PORTC5); //sets direction to up
			}
		}

   OCR0B = power;
   Task_Next();
//   radio_print ("Power is : %d\r\n", power);
 } 
}


void check_height(int grnd_dist, int des_height, int error) {

}

/*int main (int argc, char *argv[]) {

int i = 0;



//	radio_init(uint8_t channel, uint16_t address, uint8_t rx_enable);
	//uart_init();
	//clock8MHz();
	radio_init((uint8_t) 115, 0xABBA, 0 );
	init_pwm_motor(); //initialize the motor
	initTimer(); //initialize the sonar
	radio_print ("Initialized motor and sonar timers\n       ");


	for (;;) {


        pid();
		setDirection();
		for(i = 0;i<5;i++){
		_delay_ms(20);
	}

	//	printf("dist to ground: %d current error: %d accumulated error: %d \n", dist_to_ground, current_error, accumulated_error);
	//	printf("previous error: %d \n", previous_error);
	//	printf("Output: %d \n", output);


	}
	return 0;

}
*/
